'''
Created on Jan 30, 2011

@author: Donald May, Blayne Henke, Andy Kroll
'''
from QuadRotorApp import *
from Tkinter import *
import thread
import threading
import sys
from decimal import *
FPS = 100

class MainApp:
    def __init__(self, root_win, quad_app, pupdate_event):
        self.root_win = root_win
        self.quad_app = quad_app
        self.pupdate_event = pupdate_event

        self.frame = Frame(root_win, width = 300, height = 200)
        self.frame.grid(sticky = N+S+W+E)

        top = self.frame.winfo_toplevel()
        top.rowconfigure(0, weight = 1)
        top.columnconfigure(0, weight = 1)

        #return gravity vector
        grav = quad_app.get_gravity()

        self.grav_label = Label(self.frame, text = "Gravity")
        self.grav_label.grid(column = 0, row = 0, columnspan = 3, sticky = W)
        self.grav_x_string = StringVar()
        self.grav_x_entry = Entry(self.frame, width = 5, textvariable = self.grav_x_string)
        self.grav_x_string.set(str(grav[0]))
        self.grav_x_entry.bind("<Return>", self.grav_handler)
        self.grav_x_entry.grid(column = 0, row = 1)
        self.grav_y_string = StringVar()
        self.grav_y_entry = Entry(self.frame, width = 5, textvariable = self.grav_y_string)
        self.grav_y_string.set(str(grav[1]))
        self.grav_y_entry.bind("<Return>", self.grav_handler)
        self.grav_y_entry.grid(column = 1, row = 1)
        self.grav_z_string = StringVar()
        self.grav_z_entry = Entry(self.frame, width = 5, textvariable = self.grav_z_string)
        self.grav_z_string.set(str(grav[2]))
        self.grav_z_entry.bind("<Return>", self.grav_handler)
        self.grav_z_entry.grid(column = 2, row = 1)

        self.for_motor_label = Label(self.frame, text = "Front Motor")
        self.for_motor_label.grid(column = 0, row = 2, columnspan = 3,  sticky = W)

        self.front_dir = DoubleVar()
        self.front_dir.set(quad_app.get_motor_dir(0))
        self.for_clockwise = Radiobutton(self.frame, text = "Clockwise", command = self.dir_change, variable = self.front_dir, value = -1)
        self.for_clockwise.grid(column = 0, row = 3, sticky = N+S+E+W)
        self.for_counterclockwise = Radiobutton(self.frame, text = "Counter Clockwise", command = self.dir_change, variable = self.front_dir, value = 1)
        self.for_counterclockwise.grid(column = 1, row = 3, columnspan = 2, sticky = N+S+E+W)


        self.rear_motor_label = Label(self.frame, text = "Rear Motor")
        self.rear_motor_label.grid(column = 0, row = 4, columnspan = 3,  sticky = W)

        self.rear_dir = DoubleVar()
        self.rear_dir.set(quad_app.get_motor_dir(1))
        self.rear_clockwise = Radiobutton(self.frame, text = "Clockwise", command = self.dir_change, variable = self.rear_dir, value = -1)
        self.rear_clockwise.grid(column = 0, row = 5, sticky = N+S+E+W)
        self.rear_counterclockwise = Radiobutton(self.frame, text = "Counter Clockwise", command = self.dir_change, variable = self.rear_dir, value = 1)
        self.rear_counterclockwise.grid(column = 1, row = 5, columnspan = 2, sticky = N+S+E+W)



        self.right_motor_label = Label(self.frame, text = "Right Motor")
        self.right_motor_label.grid(column = 0, row = 6, columnspan = 3,  sticky = W)

        self.right_dir = DoubleVar()
        self.right_dir.set(quad_app.get_motor_dir(2))
        self.right_clockwise = Radiobutton(self.frame, text = "Clockwise", command = self.dir_change, variable = self.right_dir, value = -1)
        self.right_clockwise.grid(column = 0, row = 7, sticky = N+S+E+W)
        self.right_counterclockwise = Radiobutton(self.frame, text = "Counter Clockwise", command = self.dir_change, variable = self.right_dir, value = 1)
        self.right_counterclockwise.grid(column = 1, row = 7, columnspan = 2, sticky = N+S+E+W)

        self.left_motor_label = Label(self.frame, text = "Left Motor")
        self.left_motor_label.grid(column = 0, row = 8, columnspan = 3,  sticky = W)

        self.left_dir = DoubleVar()
        self.left_dir.set(quad_app.get_motor_dir(3))
        self.left_clockwise = Radiobutton(self.frame, text = "Clockwise", command = self.dir_change, variable = self.left_dir, value = -1)
        self.left_clockwise.grid(column = 0, row = 9, sticky = N+S+E+W)
        self.left_counterclockwise = Radiobutton(self.frame, text = "Counter Clockwise", command = self.dir_change, variable = self.left_dir, value = 1)
        self.left_counterclockwise.grid(column = 1, row = 9, columnspan = 2, sticky = N+S+E+W)

        self.start_button = Button(self.frame, text = "Start Simulation", command = self.start_simulation)
        self.start_button.grid(column = 0, row = 10)
        self.stop_button = Button(self.frame, text = "Stop Simulation", command = self.stop_simulation)
        self.stop_button.grid(column = 2, row = 10)

        self.create_menus(self.root_win)

    def create_menus(self, root):
        self.main_menu = Menu(root, tearoff = 0)
        self.file_menu = Menu(root, tearoff = 0)
        self.file_menu.add_command(label = "Exit", command = self.quit_app)
        self.main_menu.add_cascade(label = "File", menu = self.file_menu)
        root.config(menu = self.main_menu)

    def start_simulation(self):
        self.pupdate_event.clear()
        self.quad_app.start()
        self.pupdate_event.set()

    def stop_simulation(self):
        self.pupdate_event.clear()
        self.quad_app.stop()
        self.pupdate_event.set()

    def grav_handler(self, event):
        self.pupdate_event.clear()
        grav = (double(self.grav_x_string.get()), double(self.grav_y_string.get()), double(self.grav_z_string.get()))
        self.quad_app.set_gravity(grav)

        self.pupdate_event.set()

    def dir_change(self):
        self.pupdate_event.clear()
        quad_app.set_motor_dir(0, self.front_dir.get())
        quad_app.set_motor_dir(1, self.rear_dir.get())
        quad_app.set_motor_dir(2, self.right_dir.get())
        quad_app.set_motor_dir(3, self.left_dir.get())

        self.pupdate_event.set()

    def quit_app(self):
        self.pupdate_event.clear()
        self.quad_app.quit_app()
        self.root_win.destroy()


def ControlThread(quad_app = None, pupdate_event = None):
    control_win = Tk()
    control_win.title("Controls")
    main_app = MainApp(control_win, quad_app, pupdate_event)
    control_win.mainloop()

if __name__ == '__main__':
    update_event = threading.Event()
    update_event.set()
    thrust = 0.0
    yaw = 0.0
    quad_app = QuadRotorApp()

    main_thread = thread.start_new_thread(ControlThread, (quad_app, update_event))

    while not quad_app.should_exit:
        update_event.wait()

        if quad_app.get_scene().kb.keys:
            key = quad_app.get_scene().kb.getkey()

            if key == 'a':
                thrust += 0.01
            elif key == 's':
                thrust -= 0.01

            if key == 'r':
                thrust = 0.0
                yaw = 0.0
                quad_app.reset()

            if key == 'right':
                yaw += 0.1

            elif key == 'left':
                yaw -= 0.1

        quad_app.set_thrust(thrust)
        quad_app.set_yaw(yaw)
        quad_app.update(FPS)

        rate(FPS)

